/*
  MEC_AHRS.h
*/

#ifndef MEC_AHRS_h
#define MEC_AHRS_h
#include <Wire.h>
#include "Arduino.h"

class MEC_AHRS
{
  public:
    MEC_AHRS();
    void init();
	void loop(int dt);
	void config();
	float G_Dt;

	float yaw;
    float pitch;
    float roll;
	
	float accel[3];
	float magnetom[3];
	float gyro[3];
	
  private:  
  	//Math.ino
	 float Vector_Dot_Product(const float v1[3], const float v2[3]);
		 void Vector_Cross_Product(float out[3], const float v1[3], const float v2[3]);
		 void Vector_Scale(float out[3], const float v[3], float scale);
		 void Vector_Add(float out[3], const float v1[3], const float v2[3]);
		 void Matrix_Multiply(const float a[3][3], const float b[3][3], float out[3][3]);
		 void Matrix_Vector_Multiply(const float a[3][3], const float b[3], float out[3]);
		 void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll);
	
	//DCM.ino
		 void Normalize(void);
		 void Drift_correction(void);
		 void Matrix_update(void);
		 void Euler_angles(void);
	
	//Compass.ino
		 void Compass_Heading();
	  

	//Sensores.ino
		 void I2C_Init();
		 void Accel_Init();
		 void Read_Accel();
		 void Magn_Init();
		 void Read_Magn();
		 void Gyro_Init();
		 void Read_Gyro();
	
  	//Razor_AHRS.ino
    	 void read_sensors();
		 void reset_sensor_fusion();
		 void compensate_sensor_errors();
		 void check_reset_calibration_session();
		 void turn_output_stream_on();
		 void turn_output_stream_off();
		 char readChar();
	
	//Output
		 void output_angles();
		 void output_calibration(int calibration_sensor);
		 void output_sensors_text(char raw_or_calibrated);
		 void output_sensors_binary();
		 void output_sensors();	
};
#endif